#include "FourierTron.h"
#include <iostream>


using namespace std;
using namespace SeamStress;


void FourierTron::CT_FFT(const refloat_vec& input, vector<cxfloat>& output, unsigned long int N)
{
  //check that N is a power of two
  try{if((N & (N-1)) != 0){throw (char*)("third argument must be a power of two");}}
  catch(char* str){cout<<"Exception from FourierTron::CT_FFT : "<<str<<endl;throw;return;}
  //check that input.size() >= N
  try{if(input.size() < N){throw (char*)("input vector size smaller than third argument");}}
  catch(char* str){cout<<"Exception from FourierTron::CT_FFT : "<<str<<endl;throw;return;}
  
  if(output.size() < N){output.resize(N, tempval1);}
  unsigned long int M = (N>>1);
  thread_var1 = M;
  if(nthreads > M){thread_tot = M;}
  else{thread_tot = nthreads;}
  tempvecpointer1 = &output;
  retempvecpointer2 = &input;
  pins.sewStraight(&FourierTron::CT_FFT_interleave, thread_tot);
  
  CT_FFT(output, M);
  
  //calculate the log base 2 of M/2
  unsigned long int logM = 0;
  unsigned long int tM = M>>1;
  while(tM!=0){tM>>=1;logM++;}
  thread_var1 = logM;
  thread_var2 = M;
  thread_var3 = N;
  thread_var4 = M>>1;
  if(nthreads > thread_var4){thread_tot = thread_var4;}
  else{thread_tot = nthreads;}
  tempvecpointer1 = &output;
  pins.sewStraight(&FourierTron::CT_FFT_spread, thread_tot);
  output[N - thread_var4].set(output[thread_var4]);
  output[thread_var4].conj();
}


void FourierTron::CT_FFT_interleave(void* arg)
{
  unsigned long int w = (*((unsigned long int *)arg));
  
  unsigned long int max = thread_var1 - thread_tot;
  
  unsigned long int i = w;
  unsigned long int n = i<<1;
  __builtin_prefetch(&((*tempvecpointer1)[i]), 1, 3);
  __builtin_prefetch(&((*retempvecpointer2)[n]), 0, 3);
  __builtin_prefetch(&((*retempvecpointer2)[n+1]), 0, 3);
  unsigned long int ii = w + thread_tot;
  unsigned long int nn = (ii<<1);
  while(i < max)
  {
    __builtin_prefetch(&((*tempvecpointer1)[ii]), 1, 3);
    __builtin_prefetch(&((*retempvecpointer2)[nn]), 0, 3);
    __builtin_prefetch(&((*retempvecpointer2)[nn+1]), 0, 3);
    (*tempvecpointer1)[i].set((*retempvecpointer2)[n], (*retempvecpointer2)[n+1]);
    i += thread_tot;
    n = (i<<1);
    ii += thread_tot;
    nn = (ii<<1);
  }
  (*tempvecpointer1)[i].set((*retempvecpointer2)[n], (*retempvecpointer2)[n+1]);
}


void FourierTron::CT_FFT_spread(void* arg)
{
  unsigned long int w = (*((unsigned long int *)arg));
  
  unsigned long int start = w;
  if(w==0)
  {
    unsigned long int i = 0;
    unsigned long int k = 0;
    conj((*tempvecpointer1)[k], threadvec1[w]);
    sub((*tempvecpointer1)[i], threadvec1[w], threadvec2[w]);
    threadvec1[w].add((*tempvecpointer1)[i]);
    
    twiddleVal(thread_var1, i, (*tempvecpointer1)[i]);
    (*tempvecpointer1)[i].mul(threadvec2[w]);
    (*tempvecpointer1)[i].mul_by_i();
    (*tempvecpointer1)[i].neg();
    (*tempvecpointer1)[i].add(threadvec1[w]);
    (*tempvecpointer1)[i].mul_power_2(-1);
    
    k = thread_var2;
    threadvec1[w].conj();
    threadvec2[w].conj();
    twiddleVal(thread_var1, k, (*tempvecpointer1)[k]);
    (*tempvecpointer1)[k].mul(threadvec2[w]);
    (*tempvecpointer1)[k].mul_by_i();
    (*tempvecpointer1)[k].add(threadvec1[w]);
    (*tempvecpointer1)[k].mul_power_2(-1);
    
    start += thread_tot;
  }
  
  for(unsigned long int i=start;i<thread_var4;i+=thread_tot)
  {
    unsigned long int k = thread_var2 - i;
    conj((*tempvecpointer1)[k], threadvec1[w]);
    sub((*tempvecpointer1)[i], threadvec1[w], threadvec2[w]);
    threadvec1[w].add((*tempvecpointer1)[i]);
    
    twiddleVal(thread_var1, i, (*tempvecpointer1)[i]);
    (*tempvecpointer1)[i].mul(threadvec2[w]);
    (*tempvecpointer1)[i].mul_by_i();
    (*tempvecpointer1)[i].neg();
    (*tempvecpointer1)[i].add(threadvec1[w]);
    (*tempvecpointer1)[i].mul_power_2(-1);
    
    threadvec1[w].conj();
    threadvec2[w].conj();
    twiddleVal(thread_var1, k, (*tempvecpointer1)[k]);
    (*tempvecpointer1)[k].mul(threadvec2[w]);
    (*tempvecpointer1)[k].mul_by_i();
    (*tempvecpointer1)[k].add(threadvec1[w]);
    (*tempvecpointer1)[k].mul_power_2(-1);
    
    conj((*tempvecpointer1)[i], (*tempvecpointer1)[thread_var3 - i]);
    conj((*tempvecpointer1)[k], (*tempvecpointer1)[thread_var3 - k]);
  }
  
}


void FourierTron::inverseCT_FFT(const vector<cxfloat>& input, refloat_vec& output, unsigned long int N)
{
  //check that N is a power of two
  try{if((N & (N-1)) != 0){throw (char*)("third argument must be a power of two");}}
  catch(char* str){cout<<"Exception from FourierTron::CT_FFT : "<<str<<endl;throw;return;}
  //check that input.size() >= N
  try{if(input.size() < N){throw (char*)("input vector size smaller than third argument");}}
  catch(char* str){cout<<"Exception from FourierTron::CT_FFT : "<<str<<endl;throw;return;}
  
  if(output.size() < N){refloat retemp(0, prec);output.resize(N, retemp);}
  
  unsigned long int M = (N>>1);
  //calculate the log base 2 of M/2
  unsigned long int logM = 0;
  unsigned long int tM = M>>1;
  while(tM!=0){tM>>=1;logM++;}
  thread_var1 = logM;
  thread_var2 = M;
  thread_var3 = N;
  thread_var4 = M>>1;
  if(nthreads > thread_var4){thread_tot = thread_var4;}
  else{thread_tot = nthreads;}
  if(tempvec1.size() < (M+1)){tempvec1.resize((M+1), tempval1);}
  for(unsigned long int i=0;i<(M+1);i++){tempvec1[i].set(input[i]);}
  tempvecpointer1 = &tempvec1;
  pins.sewStraight(&FourierTron::inverseCT_FFT_spread, thread_tot);
  tempvec1[thread_var4].conj();
  inverseCT_FFT(tempvec1, M);
  retempvecpointer1 = &output;
  if(nthreads > thread_var2){thread_tot = thread_var2;}
  else{thread_tot = nthreads;}
  thread_var2 = M;
  thread_var3 = N;
  thread_var4 = M>>1;
  pins.sewStraight(&FourierTron::inverseCT_FFT_interleave, thread_tot);
}


void FourierTron::inverseCT_FFT_spread(void* arg)
{
  unsigned long int w = (*((unsigned long int *)arg));
  
  for(unsigned long int i=w;i<thread_var4;i+=thread_tot)
  {
    unsigned long int k = thread_var2 - i;
    
    twiddleVal(thread_var1, i, threadvec1[w]);
    threadvec1[w].conj();
    conj((*tempvecpointer1)[k], threadvec2[w]);threadvec2[w].neg();
    threadvec2[w].add((*tempvecpointer1)[i]);
    threadvec2[w].mul(threadvec1[w]);
    conj((*tempvecpointer1)[k], threadvec1[w]);
    threadvec1[w].add((*tempvecpointer1)[i]);
    
    (*tempvecpointer1)[i].set(threadvec2[w]);(*tempvecpointer1)[i].mul_by_i();
    (*tempvecpointer1)[i].add(threadvec1[w]);
    
    threadvec1[w].conj();
    threadvec2[w].conj();
    
    (*tempvecpointer1)[k].set(threadvec2[w]);(*tempvecpointer1)[k].mul_by_i();
    (*tempvecpointer1)[k].add(threadvec1[w]);
    
    (*tempvecpointer1)[k].mul_power_2(-1);
    (*tempvecpointer1)[i].mul_power_2(-1);
  }
}


void FourierTron::inverseCT_FFT_interleave(void* arg)
{
  unsigned long int w = (*((unsigned long int *)arg));
  
  for(unsigned long int i=w;i<thread_var2;i+=thread_tot)
  {
    unsigned long int n = (i<<1);
    (*tempvecpointer1)[i].get_real((*retempvecpointer1)[n]);
    (*tempvecpointer1)[i].get_imag((*retempvecpointer1)[n+1]);
  }
}


